/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#pragma once
#include <sys/time.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <boost/any.hpp>
#include <cmath>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <vector>

#include "stdlib.h"

// 定义测试模式宏
#ifdef ENABLE_TESTING
template<typename T> class TestAccess;
#define TEST_FRIEND(ClassName) friend class TestAccess<ClassName>
#else
#define TEST_FRIEND(ClassName)
#endif

namespace hitcrt {
/**
 * @description:卡尔曼滤波基类
 * @author: WWW
 * @mail: 1922039181@qq.com
 */
class KF {
    TEST_FRIEND(KF);
   public:
    KF(const int measureDim, const int stateDim)
        : m_measureDimension(measureDim), m_stateDimension(stateDim) {
        m_transitionMatrix.resize(m_stateDimension, m_stateDimension);
        m_measurementMatrix.resize(m_measureDimension, m_stateDimension);
        m_processNoiseCov.resize(m_stateDimension, m_stateDimension);
        m_measurementNoiseCov.resize(m_measureDimension, m_measureDimension);
        m_errorCovPrior.resize(m_stateDimension, m_stateDimension);
        m_statePrior.resize(m_stateDimension, 1);
        m_statePost.resize(m_stateDimension, 1);
        m_statePost = Eigen::MatrixXd::Zero(m_stateDimension, 1);
        m_measurement.resize(m_measureDimension, 1);
        m_measurement = Eigen::MatrixXd::Zero(m_measureDimension, 1);
        m_errorCovPost.resize(m_stateDimension, m_stateDimension);
        m_errorCovPost = Eigen::MatrixXd::Identity(m_stateDimension, m_stateDimension);
        m_gain.resize(m_stateDimension, m_measureDimension);
        m_residual.resize(m_measureDimension, 1);
        m_transformation.resize(m_measureDimension, 1);
    };
    ~KF(){};
    virtual void init(const std::vector<boost::any> &param) = 0;       // 设置在迭代过程中不会改变的参数
    virtual void setParam(const std::vector<boost::any> &param) = 0;   // 设置在迭代过程中会改变的参数
    virtual void predict();
    virtual void stateUpdate();
    virtual void measureUpdate();
    virtual Eigen::MatrixXd &getResult();

    bool m_conExist = false;                // 是否存在控制矩阵和控制量


   protected:
    virtual void stateUpdateImpl();
    virtual void measureUpdateImpl();
    virtual void checkStateUpdate();
    virtual void checkMeasureUpdate();
    const int m_measureDimension;           // 测量向量的维度
    const int m_stateDimension;             // 状态向量的维度
    double m_deltaTime;                     // 采样时间间隔

    Eigen::MatrixXd m_transitionMatrix;     // 状态转移矩阵
    Eigen::MatrixXd m_statePrior;           // 先验状态
    Eigen::MatrixXd m_processNoiseCov;      // 过程噪声协方差矩阵
    Eigen::MatrixXd m_errorCovPrior;        // 先验估计协方差矩阵
    
    Eigen::MatrixXd m_measurementMatrix;    // 状态向量到测量向量的转换矩阵
    Eigen::MatrixXd m_measurement;          // 测量向量   
    Eigen::MatrixXd m_measurementNoiseCov;  // 测量噪声协方差矩阵
    Eigen::MatrixXd m_gain;                 // 卡尔曼增益
    Eigen::MatrixXd m_errorCovPost;         // 后验估计协方差矩阵
    Eigen::MatrixXd m_statePost;            // 后验状态

    Eigen::MatrixXd m_residual;             // 滤波器估计的测量向量和实际测量向量的差 不一定要用上
    Eigen::MatrixXd m_transformation;       // 滤波器估计的测量向量
    // 可选的控制矩阵和控制量
    Eigen::MatrixXd m_controlledMatrix;     // 控制矩阵, 描述了控制量如何影响状态
    Eigen::MatrixXd m_controlledVec;        // 控制量

    // 输入参数异常处理类
    class paramError : public std::exception {
       public:
        paramError() = delete;
        explicit paramError(const std::string &errorMessage) { buildErrorMessage(errorMessage); }
        virtual const char *what() const noexcept override { return m_errorMessage.c_str(); }

       private:
        void buildErrorMessage(const std::string &errorMessage) {
            m_errorMessage = errorMessage;
            m_errorMessage += " Please input right param!\n";
        }
        std::string m_errorMessage;
    };

   protected:

    // 时间更新方程异常处理类
    class stateUpdateDimErr : public std::exception {
       public:
        stateUpdateDimErr() = delete;
        explicit stateUpdateDimErr(const std::string &formula) { buildErrorMessage(formula); }
        virtual const char *what() const noexcept override { return m_errorMessage.c_str(); }

       private:
        void buildErrorMessage(const std::string &formula) {
            m_errorMessage = "StateUpdate():The ";
            m_errorMessage += formula;
            m_errorMessage += " formula is false!! Matrix dimensions must agree!\n";
        }
        std::string m_errorMessage;
    };
    // 测量更新方程异常处理类
    class measUpdateDimErr : public std::exception {
       public:
        measUpdateDimErr() = delete;
        explicit measUpdateDimErr(const std::string &formula) { buildErrorMessage(formula); }
        virtual const char *what() const noexcept override { return m_errorMessage.c_str(); }

       private:
        void buildErrorMessage(const std::string &formula) {
            m_errorMessage = "MeasUpdate():The ";
            m_errorMessage += formula;
            m_errorMessage += " formula is false!! Matrix dimensions must agree!\n";
        }
        std::string m_errorMessage;
    };
};
}  // namespace hitcrt

